#include <iostream>
#include <fstream>
#include <opencv.hpp>
#include "helpers.h"

using namespace std;
using namespace cv;

int main()
{
	Mat p2d, p3d;

	p2d = load2dMat("files/pts2d-pic_b.txt", 0); // 1 means load normalized points, 0 means real points
	p3d = load3dMat("files/pts3d.txt", 0);
	//p2d = load2dMat("files/pts2d-norm-pic_a.txt", 0); // 1 means load normalized points, 0 means real points
	//p3d = load3dMat("files/pts3d-norm.txt", 0);
	//Mat trans = findTransform(p2d);

	Mat m(3, 4, CV_64F);
	m.at<double>(0,0) = 3.103525e-01;
	m.at<double>(0,1) = -8.030680e-01;
	m.at<double>(0,2) = -6.320847e-03;
	m.at<double>(0,3) = -2.276153e-02;
	m.at<double>(1,0) = -3.003717e-01;
	m.at<double>(1,1) = -6.628133e-02;
	m.at<double>(1,2) = -6.101744e-01;
	m.at<double>(1,3) = -9.510487e-02;
	m.at<double>(2,0) = 4.713473e-01;
	m.at<double>(2,1) = 2.094120e-01;
	m.at<double>(2,2) = -3.736182e-02;
	m.at<double>(2,3) = 1.000000e+00;

	/*
	m.at<double>(0,0) = -.4583;
	m.at<double>(0,1) = .2947;
	m.at<double>(0,2) = .0139;
	m.at<double>(0,3) = -0.0040;
	m.at<double>(1,0) = .0509;
	m.at<double>(1,1) = .0546;
	m.at<double>(1,2) = .5410;
	m.at<double>(1,3) = .0524;
	m.at<double>(2,0) = -.1090;
	m.at<double>(2,1) = -.1784;
	m.at<double>(2,2) = .0443;
	m.at<double>(2,3) = -.5968;*/

	//cout << matToString(m) << endl;

	//transforms
	Mat t2 = findTransform(p2d);
	Mat t3 = findTransform(p3d);
	//Mat m1 = findBestM(p2d, p3d, t2, t3);
	//Mat c = findCenter(m, t3);
	Mat c = findCenter(m, t3);
	cout << matToString(c) << endl;

	//cout << matToString(c) << endl;



	Mat a1(3,1, CV_64F);
	Mat b1(3,1, CV_64F);
	a1.at<double>(0,0) = 0.0;
	a1.at<double>(1,0) = 0.0;
	a1.at<double>(2,0) = 1.0;
	b1.at<double>(0,0) = 3.0;
	b1.at<double>(1,0) = 4.0;
	b1.at<double>(2,0) = 1.0;


	//Mat m = findBestM(p2d, p3d);

	return 0;
}
